#ifndef GAMEENGINE_GEOMETRY_SPHERE_HPP
#define GAMEENGINE_GEOMETRY_SPHERE_HPP

#include <GameEngine/Geometry/Location/vector3d.h>

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the sphere class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Sphere<T>::Sphere() {}

//! Constructor from a center and a radius
/*!
 @param center The center of the sphere
 @param radius The radius of the sphere
 @note Class T should have a copy constructor
 */
template <class T>
Sphere<T>::Sphere(const Point3d<T>& center, const T& radius) : center_(center), radius_(radius) {}

//! Copy constructor
/*!
 @param rhs The sphere to copy
 @note Class T should have a copy constructor
 */
template <class T>
Sphere<T>::Sphere(const Sphere& rhs) : center_(rhs.center_), radius_(rhs.radius_) {}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Sphere<T>::~Sphere() {}

//! Assignment operator
/*!
 @param rhs The sphere to copy
 @note Class T should have an assignment operator
 */
template <class T>
Sphere<T>& Sphere<T>::operator=(const Sphere& rhs) {
	if ( &rhs != this ) {
		center_ = rhs.center_;
		radius_ = rhs.radius_;
	}
	return *this;
}

//! Accessor to the center of the sphere (read-write)
/*!
 @return A reference to the center of the sphere
 */
template <class T>
Point3d<T>& Sphere<T>::center() {
	return center_;
}

//! Accessor to the center of the sphere (read only)
/*!
 @return A constant reference to the center of the sphere
 */
template <class T>
const Point3d<T>& Sphere<T>::center() const {
	return center_;
}

//! Accessor to the radius of the sphere (read-write)
/*!
 @return A reference to the radius of the sphere
 */
template <class T>
T& Sphere<T>::radius() {
	return radius_;
}

//! Accessor to the radius of the sphere (read only)
/*!
 @return A constant reference to the radius of the sphere
 */
template <class T>
const T& Sphere<T>::radius() const {
	return radius_;
}

//! Compute the diameter of the sphere
/*!
 @return The diameter of the sphere, i.e., 2*radius
 */
template <class T>
T Sphere<T>::diameter() const {
	return (T)(2.0*radius_);
}

//! Compute the area of the sphere
/*!
 @return The area of the sphere, i.e., 4*pi*radius*radius
 */
template <class T>
T Sphere<T>::area() const {
	return (T)(4.0*PI*radius_*radius_);
}

//! Compute the volume of the sphere
/*!
 @return The volume of the sphere, i.e., 4*pi*radius*radius*radius/3
 */
template <class T>
T Sphere<T>::volume() const {
	return (T)(4.0*PI*radius_*radius_*radius_ / 3.0);
}

//! Test wether a point is inside the sphere
/*!
 @param rhs The point to test for inclusion
 @param strict If set to true, a point located exactly on the perimeter of
			   the sphere is not considered to belong to the sphere
 @return True if the point is inside the sphere, false otherwise
 */
template <class T>
bool Sphere<T>::inside(const Point3d<T>& rhs, bool strict) const {
	// Get the square distance between the points
	T square_dist = center_.get_square_distance(rhs);

	// Compare with the square radius to test point inclusion
	if ( strict ) return ( square_dist < (radius_*radius_) );
	return ( square_dist <= (radius_*radius_) );
}

//! Compute the intersection between the line and the sphere
/*!
 @param rhs The line to intersect with the sphere
 @param coordinate1 The parametric coordinate of the first intersection between
                    the sphere and the line, if both do intersect. To retrieve
					the intersection point, call rhs.point(coordinate1)
 @param coordinate2 The parametric coordinate of the second intersection between
                    the sphere and the line, if both do intersect. To retrieve
					the intersection point, call rhs.point(coordinate2)
 @param type If not nil, this parameter receives the type of intersection
             between the sphere and the line. The intersection type can be:<BR>
			 NoIntersection if the line does not intersect the sphere,<BR>
			 TangentIntersection if the line is tangent to the sphere,<BR>
			 Intersection if the line fully intersects the sphere,<BR>
			 DegenerateIntersection if the line is degenerate (i.e. that the
			 first and second points of the line are the same).
 @return The number of intersections between the line and the sphere. Returned
         value can be 0 (NoIntersection or DegenerateIntersection), 1
		 (TangentIntersection) or 2 (Intersection)
 @note Type T should have an implicit conversion to type double
 */
template <class T>
int Sphere<T>::intersection(const Line3d<T>& rhs, T& coordinate1, T& coordinate2, IntersectionType::Type* type) const {
	// Check that line is not degenerate
	T square_length = rhs.p1().get_square_distance(rhs.p2());
	if ( square_length <= 0 ) {
		if ( type ) *type = IntersectionType::DegenerateIntersection;
		return 0;
	}

	// Compute the determinant
	T det = (T)0;
	{{
		T det1 = (
			  rhs.p1().x()*rhs.p1().x()
			+ rhs.p2().x()*center_.x()
			- rhs.p1().x()*(rhs.p2().x() + center_.x())
			+ rhs.p1().y()*rhs.p1().y()
			+ rhs.p2().y()*center_.y()
			- rhs.p1().y()*(rhs.p2().y() + center_.y())
			+ (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - center_.z())
		);
		T det2 = (
			  (rhs.p1().x() - rhs.p2().x())*(rhs.p1().x() - rhs.p2().x())
			+ (rhs.p1().y() - rhs.p2().y())*(rhs.p1().y() - rhs.p2().y())
			+ (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - rhs.p2().z())
		);
		T det3 = (
			  -radius_*radius_
			+ (rhs.p1().x() - center_.x())*(rhs.p1().x() - center_.x())
			+ (rhs.p1().y() - center_.y())*(rhs.p1().y() - center_.y())
			+ (rhs.p1().z() - center_.z())*(rhs.p1().z() - center_.z())
		);
		det = det1*det1 - det2*det3;
	}}

	// Check whether there is an intersection
	if ( det < 0 ) {
		if ( type ) *type = IntersectionType::NoIntersection;
		return 0;
	}

	// Compute intersection(s)
	T sq_det = (T)sqrt((double)det);
	T num =   rhs.p1().x()*rhs.p1().x() - rhs.p1().x()*rhs.p2().x() - rhs.p1().x()*center_.x() + rhs.p2().x()*center_.x()
		    + rhs.p1().y()*rhs.p1().y() - rhs.p1().y()*rhs.p2().y() - rhs.p1().y()*center_.y() + rhs.p2().y()*center_.y()
		    + rhs.p1().z()*rhs.p1().z() - rhs.p1().z()*rhs.p2().z() - rhs.p1().z()*center_.z() + rhs.p2().z()*center_.z();
	coordinate1 = ( num + sq_det ) / square_length;
	coordinate2 = ( num - sq_det ) / square_length;

	// Output intersection type
	if ( det == 0 ) {
		if ( type ) *type = IntersectionType::TangentIntersection;
		return 1;
	}
	if ( type ) *type = IntersectionType::Intersection;
	return 2;
}

//! Get the point along the sphere at given azimuth and dip
/*!
 @param azimuth The azimuth of the point to compute. Azimuth is expected to be
                in [0,2*pi]. North direction is taken at azimuth 0, and azimuths
				are considered clockwise
 @param dip The dip of the point to compute. Dip is expected to be in [-pi/2,pi/2].
            Horizontal direction is taken at dip 0, dips in ]0,pi/2] correspond to
			the upper part of the sphere, and dips in [-pi/2,0[ correspond to the
			lower part of the sphere
 @return The point at given azimuth and dip
 @note Type T should have an implicit conversion with type double
 @note Results are unpredictable if angles are not in the correct ranges, i.e.,
       [0,2*pi] for the azimuth and [-pi/2,pi/2] for the dip
 */
template <class T>
Point3d<T> Sphere<T>::point(const Angle<T>& azimuth, const Angle<T>& dip) const {
	return Point3d<T>(
				center_.x() + radius_*(T)( cos((double)azimuth.rad()) * cos((double)dip.rad()) ),
				center_.y() + radius_*(T)( sin((double)azimuth.rad()) * cos((double)dip.rad()) ),
				center_.z() + radius_*(T)sin((double)dip.rad())
			);
}

//! Compute the azimuth and dip of the point along the sphere
/*!
 @param azimuth The azimuth of the point
 @param dip The dip of the point
 @param length The distance between the point and the sphere center
 @param rhs The point to compute the azimuth and dip
 */
template <class T>
void Sphere<T>::get_azimuth_and_dip(Angle<T>& azimuth, Angle<T>& dip, T& length, const Point3d<T>& rhs) const {
	// Compute length
	length = rhs.get_distance(center_);

	// Compute dip
	Vector3d<T> vec(center_, rhs); vec.normalize();
	dip = Angle<T>((T)asin((double)vec.z()), GameEngine::Angles::Rad);

	// Compute azimuth
	vec.z() = (T)0; vec.normalize();
	azimuth = Angle<T>((T)asin((double)vec.y()), GameEngine::Angles::Rad);
}

//! Compute the azimuth and dip of the closest point along the sphere
/*!
 @param azimuth The azimuth of the closest point
 @param dip The dip of the closest point
 @param rhs The point to project onto the sphere
 @note If you want to get back to the point location, call the
       sphere.point(azimuth,dip) function
 */
template <class T>
void Sphere<T>::closest_point(Angle<T>& azimuth, Angle<T>& dip, const Point3d<T>& rhs) const {
	// Get the closest point location
	Point3d<T> pt = closest_point(rhs);

	// Compute the azimuth and dip of the intersection
	T length;
	get_azimuth_and_dip(azimuth, dip, length, pt);
}

//! Compute the closest point along the sphere
/*!
 @param rhs The point to project onto the sphere
 @return The location of the closest point along the sphere
 */
template <class T>
Point3d<T> Sphere<T>::closest_point(const Point3d<T>& rhs) const {
	// Create a line between the sphere center and the given point
	Line3d<T> line(center_, rhs);

	// Compute the intersection between this line and the sphere
	T coordinate1, coordinate2;
	int res = intersection(line, coordinate1, coordinate2);
	if ( coordinate1 < 0 ) coordinate1 = coordinate2;

	// Return result
	return line.point(coordinate1);
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
